# -*- coding: utf-8 -*-
"""
Created on Wed Sep  7 11:50:04 2022

@author: wulong
"""
import numpy as np
from casadi import *
from Slow_formulate_Opt import *
from Obtain_results import *

def S_EMPC(initial_state, state_guess, state_bnds_lo, state_bnds_up,
           fast_state_guess, fast_state_bnds_lo, fast_state_bnds_up,
           input_guess, input_bnds_lo, input_bnds_up,
           fast_input_guess, fast_input_bnds_lo, fast_input_bnds_up,
           input_bin_bnds,
           distb_bnds,
           output_guess,
           yspe_guess,
           yspe_2_bnds_lo, yspe_2_bnds_up, 
           output_bnds_lo, output_bnds_up, long_state_setpnts,
           alpha, pred_horzn, pf, pmg, pse,
           i_s, X_int_f, 
           i, del_Uc, del_Ucf, past_Uc, past_Ucf, past_Uz,
           pcm, ppn, rxi, rxi_as, yeb):
    
    state_guess_new = np.append(np.delete(state_guess, 0, axis=0),
                                state_guess[-1,:].reshape(1,-1), axis=0)
    
    fast_state_guess_new = np.append(np.delete(fast_state_guess, 0, axis=0),
                                     fast_state_guess[-1,:].reshape(1,-1), axis=0)
    
    input_guess_new = np.append(np.delete(input_guess, 0, axis=0),
                                input_guess[-1,:].reshape(1,-1), axis=0)
    
    fast_input_guess_new = np.append(np.delete(fast_input_guess, 0, axis=0),
                                     fast_input_guess[-1,:].reshape(1,-1), axis=0)
    
    output_guess_new = np.append(np.delete(output_guess, 0, axis=0),
                                 output_guess[-1,:].reshape(1,-1), axis=0)
    
    yspe_guess_new = np.append(np.delete(yspe_guess, 0, axis=0),
                               yspe_guess[-1,:].reshape(1,-1), axis=0)
    
    distb_bnds_new = distb_bnds[i_s:i_s+pred_horzn,:]
    
    yspe_bnds_lo_new = yspe_2_bnds_lo[i_s:i_s+pred_horzn]
    yspe_bnds_up_new = yspe_2_bnds_up[i_s:i_s+pred_horzn]
    
    long_state_setpnts_new = long_state_setpnts[i_s:i_s+pred_horzn,:]
    
    input_bin_bnds_new = input_bin_bnds[i_s:i_s+pred_horzn,:]
    
    pse_new = pse[i_s:i_s+pred_horzn]
    pcm_new = pcm[i_s:i_s+pred_horzn]
    ppn_new = ppn[i_s:i_s+pred_horzn]
    rxi_new = rxi[i_s:i_s+pred_horzn]
    rxi_as_new = rxi_as[i_s:i_s+pred_horzn]
    
    yeb_new = yeb[i_s:i_s+pred_horzn]
    
    f_opt = formulate_opt_s(initial_state, state_guess_new, state_bnds_lo, state_bnds_up,
                            fast_state_guess_new, fast_state_bnds_lo, fast_state_bnds_up,
                            input_guess_new, input_bnds_lo, input_bnds_up,
                            fast_input_guess_new, fast_input_bnds_lo, fast_input_bnds_up,
                            input_bin_bnds_new,
                            distb_bnds_new,
                            output_guess_new,
                            yspe_guess_new, yspe_bnds_lo_new, yspe_bnds_up_new,
                            output_bnds_lo, output_bnds_up, long_state_setpnts_new,
                            alpha, pred_horzn, pf, pmg, pse_new,
                            X_int_f,
                            i, del_Uc, del_Ucf, past_Uc, past_Ucf, past_Uz,
                            pcm_new, ppn_new, rxi_new, rxi_as_new, yeb_new)
    
    s_opt = solve_opt_s(*f_opt)
    
    r_opt = get_results_s(s_opt, pred_horzn)
    
    return r_opt
